#include "ToyRenderer.h"

namespace ToyCube {

Mat<4, 4, float> RotateMat(float x, float y, float z, float theta){
    
    Mat<4, 4, float> mat;
    mat.SetIdentify();
    
    float qsin = (float)sin(theta * 0.5f);
    float qcos = (float)cos(theta * 0.5f);
    float w = qcos;
    Vec<3, float> vec({x, y, z});
    vec = Normalize(vec);
    x = vec[0] * qsin;
    y = vec[1] * qsin;
    z = vec[2] * qsin;
  
    mat.m[0][0] = 1 - 2 * y * y - 2 * z * z;
    mat.m[0][1] = 2 * x * y - 2 * w * z;
    mat.m[0][2] = 2 * x * z + 2 * w * y;
    mat.m[1][0] = 2 * x * y + 2 * w * z;
    mat.m[1][1] = 1 - 2 * x * x - 2 * z * z;
    mat.m[1][2] = 2 * y * z - 2 * w * x;
    mat.m[2][0] = 2 * x * z - 2 * w * y;
    mat.m[2][1] = 2 * y * z + 2 * w * x;
    mat.m[2][2] = 1 - 2 * x * x - 2 * y * y;
    mat.m[3][0] = mat.m[3][1] = mat.m[3][2] = 0.0f;
    mat.m[0][3] = mat.m[1][3] = mat.m[2][3] = 0.0f;
    mat.m[3][3] = 1.0f;
    
    return mat;
}

Mat<4, 4, float> SacleMat(float x, float y, float z){
    Mat<4, 4, float> mat;
    mat.SetIdentify();
    mat.m[0][0] *= x;
    mat.m[1][1] *= y;
    mat.m[2][2] *= z;
    return mat;
}

Mat<4, 4, float> TranslateMat(float x, float y, float z){
    Mat<4, 4, float> mat;
    mat.SetIdentify();
    mat.m[0][3] = x;
    mat.m[1][3] = y;
    mat.m[2][3] = z;
    return mat;
}

Mat<4, 4, float> CameraMat(const Vec<3, float> &up, const Vec<3, float> &eye, const Vec<3, float> &at){
    Mat<4, 4, float> mat;

    //-z轴方向
    Vec<3, float> neg_tmp_z = SubVec(at, eye);
    std::cout<<"neg_tmp_z"<<std::endl;
    neg_tmp_z.Print();
    //x轴方向
    Vec<3, float> tmp_x = CrossMutVec3(neg_tmp_z, up);
    std::cout<<"tmp_x"<<std::endl;
    tmp_x.Print();
    //y轴方向
    Vec<3, float> tmp_y = CrossMutVec3(tmp_x, neg_tmp_z);
    std::cout<<"tmp_y"<<std::endl;
    tmp_y.Print();
    
    neg_tmp_z = Normalize(neg_tmp_z);
    tmp_x = Normalize(tmp_x);
    tmp_y = Normalize(tmp_y);
    
    Mat<4, 4, float> trans({{1, 0, 0, -eye[0]},
                            {0, 1, 0, -eye[1]},
                            {0, 0, 1, -eye[2]},
                            {0, 0, 0, 1}});
    std::cout<<"trans"<<std::endl;
    trans.Print();
    
    //整体移动世界坐标 -> 将摄像机放到原点 左右旋转与up垂直 看向z的负方向(右手系)
    //也就是将偏转了的 neg_z x y 移到up决定z轴方上旋转角度 看向z轴负方向
    //neg_z x y 三者正交 则其逆矩阵就是其转置矩阵
    Mat<4, 4, float> rotate({{tmp_x[0], tmp_x[1], tmp_x[2], 0},
                             {tmp_y[0], tmp_y[1], tmp_y[2], 0},
                             {neg_tmp_z[0], neg_tmp_z[1], neg_tmp_z[2], 0},
                             {0, 0, 0, 1}});
    
    std::cout<<"rotate"<<std::endl;
    rotate.Print();
    
    mat.SetIdentify();
    //先平移 后旋转
    rotate.RightMut(trans);
        
    return rotate;
}

Mat<4, 4, float> OrthProjMat(float eye_fov, float aspect, float near, float far){
    
    Mat<4, 4, float> mat;
    mat.SetIdentify();
    
    float top = near * tan(PI*(eye_fov/2)/180.0);
    float right = top * aspect;
    float left = -right;
    float bottom = - top;
    
    //将长宽高归一化到 -1~1
    Mat<4, 4, float> orth_normalize({{2/(right-left), 0, 0, 0},
                                     {0, 2/(top-bottom), 0, 0},
                                     {0, 0, 2/(near-far), 0}, //右手系near大于far
                                     {0, 0, 0, 1}});
    //将归一化好的正方体中心平移到坐标中点
    Mat<4, 4, float> orth_trans({{1, 0, 0, -(right+left)/2},
                                 {0, 1, 0, -(top+bottom)/2},
                                 {0, 0, 1, -(near+far)/2},
                                 {0, 0, 0, 1}});
    
    //先归一化 再平移到中点
    orth_trans.RightMut(orth_normalize);
    mat.RightMut(orth_trans);
    
    return mat;
}

Mat<4, 4, float> PersProjMat(float eye_fov, float aspect, float near, float far){
    
    Mat<4, 4, float> mat;
    mat.SetIdentify();
    
    //视锥体压缩为长方体
    //将空间内的点x,y方向投影到近平面反推变换矩阵p2o
    //第三行带入近平面所有点以及远平面z轴上的点经过压缩后不变的特点待定系数法求得
    Mat<4, 4, float> p2o({{near, 0, 0, 0},
                          {0, near, 0, 0},
                          {0, 0, (near+far), -(near*far)},
                          {0, 0, 1, 0}});
    
    std::cout<<"P2O"<<std::endl;
    p2o.Print();
    
    float top = near * tan(PI*(eye_fov/2)/180.0);
    float right = top * aspect;
    float left = -right;
    float bottom = - top;
    
    //将长宽高归一化到 -1~1
    Mat<4, 4, float> orth_normalize({{2/(right-left), 0, 0, 0},
                                     {0, 2/(top-bottom), 0, 0},
                                     {0, 0, 2/(near-far), 0}, //右手系near大于far
                                     {0, 0, 0, 1}});
    
//    std::cout<<"orth_normalize"<<std::endl;
//    orth_normalize.Print();
    
    //将归一化好的正方体中心平移到坐标中点
    Mat<4, 4, float> orth_trans({{1, 0, 0, -(right+left)/2},
                                 {0, 1, 0, -(top+bottom)/2},
                                 {0, 0, 1, -(near+far)/2},
                                 {0, 0, 0, 1}});
    
//    std::cout<<"orth_trans"<<std::endl;
//    orth_trans.Print();
    
    //平移到中点 后压缩的视锥体归一化
    orth_normalize.RightMut(orth_trans);
    //对于将视锥体压缩为立方体的空间 进行上一步变换
    orth_normalize.RightMut(p2o);
    mat.RightMut(orth_normalize);
    
//    std::cout<<"mat"<<std::endl;
//    mat.Print();
    
    return mat;
}

Vec<4, float> TransformCoordinate(Mat<4, 4, float> &mat, Vec<4, float> &vec){
    //矩阵对顶点做变换 -> 4x4矩阵 x 列向量(x,y,z,w)
    Vec<4, float> ret;
    for(size_t i=0; i<4; i++){
        for(size_t j=0; j<4; j++){
            ret.m[i] += mat.m[i][j] * vec[j];
        }
    }
    return ret;
}

}
